How to Estimate Attitude from Vector Observations 
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Wahba’s Problem - SIAM Review , July 1965 

Find the orthogonal matrix A with determinant +1 that minimizes 
L(A) = ^ i a i \b i -Ar i f. 

where {b,l are unit vectors in a body frame, {r,} are unit vectors 
in a reference frame, and {a,} are non-negative weights. Writing 

L{A) = A 0 - tr(A5 T ), with A 0 = and B = J Ji a i b i r^ , 
it is clear that we can minimize L(A ) by maximizing tr (AB T ). 

This is equivalent to the orthogonal Procrustes problem, which is 
to find the orthogonal matrix A that is closest to B in the sense of 
the Frobenius norm, ||M||p = 2,. -M- = tr(MM T ), since 

|A - B||p = ||A||p + 1 B||p - 2tr( AB t ) = 3 + ||B||p - 2tr(AB T ) 



First Solutions - SIAM Review , July 1966 

1 : J. L. Farrell and J. C. Stuelpnagel: 

B has the polar decomposition B = WH where W is orthogonal 
and H is symmetric and positive semidefinite. 

If det W is positive, then A opt = W. 

Else, diagonalize H by H = VDV T , where V is orthogonal and 
D is diagonal with elements arranged in decreasing order. 

Then A opl = WV diag[l 1 detW]V T . 

2: R. H. Wessner: 

A opt = ( B T y l (B T B ) m , which is equivalent to A opt = B{B T B)~ m . 
This requires 3 vectors (detZ? ^ 0); only 2 are really needed. 
3-6: J. R. Velman, J. E. Brock, R. Desjardins, and Wahba. 



Singular Value Decomposition (SVD) Method - 1987 

B has the Singular Value Decomposition: 

B = UZV t =U diag[X,, S 22 Z J3 ] V T , 

where U and V are orthogonal and Z n > S 22 > E 33 > 0. 

The optimal attitude matrix is A opt = t/diag[l 1 (det£/)(detV)] V T . 

The SVD method is completely equivalent to the Farrell and 
Stuelpnagel solution with U = WV. The difference is that SVD 
algorithms exist now and are among the most robust numerical 
algorithms. MATLAB computes A opt from B in two lines of code. 



QUaternion ESTimator (QUEST) - 1978 

The first three rows of (A max I -K )q opl = 0 give 

xl where x = {adj[(A max + tr B)1 - B - B T ] }z, 

Y J and y = det[(A max + tr B)I - B- B r ] 

Find A max by Newton-Raphson iteration of the characteristic eqn., 

det( A max I -K) = (A max - tr B)y - z T x = 0. 

Iterate from Aq, since A max is very close to Aq if L(A opt ) = A 0 - A max 
is small. The analytic solution is slower and no more accurate. 

QUEST would fail for 180° rotations, but the method of sequential 
rotations (effectively permuting q components) handles this case. 




Estimator of the Optimal Quaternion (ESOQ) - 1996 


Solve the characteristic equation for A max as in QUEST. 

The adjoint of A max I - K can be shown analytically to obey 
adj(A m „ /- K ) = (X am - A 2 )(A max - A,)(A max - A 4 ) 9opt ^ pl> 

where A^ A 3 , and A 4 are the other three eigenvalues of K. 

Thus q opl can be computed by normalizing any non-zero column of 
adj(A max I - K). This is the “4-dimensional cross-product” of the 

other three columns of A max I - K. 



ESOQ2 - 1997 


resin(0/2)l 

Substituting q opl = into (A max / - K )q op{ = 0 gives 

( A max - tr B) cos(0 / 2) = e T z sin(0 / 2) and 

[(A max + tr/?)/-Z?-Z? T ]esin(0/2) = zcos(0/2). 

Eliminating the rotation angle 0 gives Me = 0, where 


M = (A max - tr5)[(A max + trB)I - B - B J ]-zz T = [nij m 2 m 3 ]. 
The rotation axis is e = y/|y|, where y is any m ; xm .. Then 



1 

(A max -tr5)y| 2 +(z-y) 2 




Fast Optimal Attitude Matrix (FOAM) - 1993 

Find A max by solving the characteristic equation 

0 = (A 2 - 1 B||p ) 2 -8AdetB-4||adjB|| 2 . 

This becomes an easily solved quadratic in A 2 if detB = 0, as in 
the case of two observations. The attitude matrix is given by 

A, P , = ~ del B) \(k + ||B|| 2 f )B + A max adjB r - BB T B\ 

where k = “WIf)- 

For the analysis in this paper, the quaternion representation of the 
optimal attitude is computed. 

ESOQ, ESOQ2, and FOAM avoid QUEST’S sequential rotations. 



Two-Observation Case 


In this case det B = 0, the odd terms in A in the characteristic 
equation vanish, and 

A m „ = V a ? +<2 2 + 2 a l o 2 [(b| • b 2 )(r, -r 2 ) + |b, xb 2 ||r, xr 2 |] . 

This simplifies both QUEST and FOAM; FOAM gives 
A,p, = b 3 r 3 r + («iM nl »)[b 1 r 1 T +(b, xb 3 )(r, xr 3 ) T ] 

+ («2 M™, )[b 2 r 2 T + < b 2 xb 3 )(r 2 xr 3 ) T ], 
where b 3 = (bj x b 2 )/|b 1 x b 2 | and r 3 = (r t x r 2 )/|rj x r 2 . 

This goes over to the TRIAD solution for a x = 0, a 2 = 0, or a x = a 2 . 



Sequential Methods 

The basic idea is to propagate B or K to time t and then update. 
Filter QUEST -1989 

k+n t 

B( new) = iu<& 3x3 B(o\d)+ ^ a i b 1 .r; T , sum over n t observations at t. 

i=k+\ 


Recursive QUEST (REQUEST) -1996 

k+n t 

K( new) = /< < l) 1 . 4 /f(old)'l>[_ ) + £ a : K s , where 


K: = 


b, r ,' + r ,b,' -(b, r,)/ 

(b,xi)) T 


i=k + 1 

(b j X Tj ) 

b, r i 


These are mathematically equivalent. Filter QUEST requires 
fewer computations, but neither has been successful in practice. 



Reynolds’s Sequential Algorithm - 1997 

There are two orthogonal quaternions that map a vector r, into b,: 


<7i = 


2(1 + b,- • r ( ) [1 + b ; - -r t 


b, x r ; 


and q 2 = 


2(1 + b ( • r- ) 


b i + 1 7 


These span the subspace of 4D quaternion space consistent with 
this measurement. The projection matrix onto this 2D subspace is 

rp rp ^ 

W\ +<?2^2 =W + K i)- 

We update the quaternion by q (+ ) = (7 + rfK^qi -)/ (/ + rjK^qt -) , 
where 77 = 1 for perfect measurements, and 0 < t] < 1 for filtering. 



Star Camera Attitude Determination (SCAD) - 1998 

Write r- = r + (r { - - r) with r = ( £,- <2 t - ), and similarly for b,. 

Then L(A) = (^-ajb - Ar| 2 -b)- A(r- -r)| 2 . 

For small-field-of-view sensors, the second term is much smaller 

than the first. The general quaternion minimizing the first term is 
q(y/) = q^osiiff/l) + g 2 sin( ty/72) 

where q x and q 2 are the two quaternions that map r/|r| into b/|b . 
Then an arctangent gives the 1/r that minimizes the second term. 

Computation of r and b makes SCAD fairly slow. 



Testing 

MATLAB versions of the q method, the SVD method, QUEST, 
ESOQ, ESOQ2 and FOAM were tested. The q method used eig 
and the SVD method used svd. The other methods used 0, 1, or 
iterations of the characteristic equation to compute A max . 

Three test scenarios were simulated: 

Star tracker scenario: 5 stars in narrow field-of-view star tracker, 
6 arcsecond per star per axis measurement errors 

Unequal weights: one measurement with 1 arcsecond 
measurement noise, two with 1 ° measurement noise 

Mismodeled weights: two measurements with 0.1° measurement 
noise and one with 1°, all weighted equally. 

Each scenario was tested with 1000 random attitude matrices. 



Accuracy Results 

All algorithms performed equally well in the star tracker scenario 

The q method, the SVD method, and FOAM performed well in 
the unequal weight scenario. The iterative refinement of A max 
failed in QUEST, ESOQ, and ESOQ2 in this scenario. 

A single update of A max is required for best performance in the 
mismodeled weight scenario (except in the q and SVD methods). 

A second update of A max may improve the agreement of the 
estimate with the optimal {q and SVD method) attitude, but it 
never improves the agreement with the (simulated) true attitude. 

Special first-order variants of ESOQ (ESOQF1) and ESOQ2 
(ESOQ2.1) were developed to take advantage of this observation 



floating point operations 


Timing of Slower Methods 




Timing of Faster Methods 




Summary 

The most robust estimators minimizing Wahba’s loss function are 
Davenport’s q method and the SVD method. The q method is 
faster than the SVD method with three or more measurements. 

The other algorithms are less robust since they solve the 
characteristic polynomial equation to find the maximum 
eigenvalue of Davenport’s K matrix. They are only preferable 
when speed or processor power is an important consideration. 

Of these, FOAM is the most robust and faster than the q method. 

Robustness is only an issue for measurements with widely 
differing accuracies, so the fastest algorithms, QUEST, ESOQ, 
and ESOQ2, are well suited to star sensor applications. 



